#pragma once

#include <Eigen/Dense>
#include <cmath>
#include "wheels2D.h"

using namespace std;
using std::chrono::high_resolution_clock;
using std::chrono::milliseconds;


namespace vehicle_kinematics
{
/**
 * 假设 vx, vy, w是车体中心的速度和角速度， px, py, theta是轮子相对于车体中心的位置和角度, vp是轮子的线速度
 * 
 * 矩阵  Fi =[ vx * cos(theta) + vy * sin(theta) + (px *sin(theta) - py*cos(theta))*w - vp,
 *          vx * sin(theta) + vy * cos(theta) + (px *cos(theta) - py*sin(theta))*w]  表示 轮子速度的差异
 * 代价函数为   cost = Fi转置 * Fi
 * 经过数学计算有：
 * d(Fi)/dvx = [cos(theta), sin(theta)]T := Ai
 * d(Fi)/dvy = [sin(theta), cos(theta)]T := Bi
 * d(Fi)/dw = [px*sin(theta) - py*cos(theta), px*cos(theta) - py*sin(theta)]T := Ci
 * d(cost)/dvx = 2 * Fi转置 * d(Fi)/dvx = 2*Fi转置 * Ai
 * 同理: d(cost)/dvy = 2 * Fi转置 * d(Fi)/dvy = 2*Fi转置 * Bi, d(cost)/dw = 2 * Fi转置 * d(Fi)/dw = 2*Fi转置 * Ci
 * 
 * 继续计算二次微分，海塞矩阵
 * d2(Fi)/dvxdvx = 2* Ai转置 * Ai, d2(Fi)/dvxdvy = 2 * Bi转置 *Ai,  d2(Fi)/dvxdw = 2 * Ci转置 *Ai
 * d2(Fi)/dvydvy = 2* Bi转置 * Bi, d2(Fi)/dvydvx = 2 * Ai转置 *Bi,  d2(Fi)/dvydw = 2 * Ci转置 *Bi
 * d2(Fi)/dwdw   = 2* Ci转置 * Ci, d2(Fi)/dwdvx = 2 * Ai转置 *Ci,  d2(Fi)/dwdvy = 2 * Bi转置 *Ci
*/
inline Eigen::Vector2d calcAi(double vx, double vy, double w, double px, double py, double theta, double vp)
{
    Eigen::Vecotor2d A  << std::cos(theta), std::sin(theta);
    return A;
}

inline Eigen::Vector2d calcBi(double vx, double vy, double w, double px, double py, double theta, double vp)
{
    Eigen::Vector2d B  << std::sin(theta), std::cos(theta);
    return B;
}

inline Eigen::Vector2d calcCi(double vx, double vy, double w, double px, double py, double theta, double vp)
{
    Eigen::Vector2d C  << px * std::sin(theta) - py * std::cos(theta), px * std::cos(theta) - py * std::sin(theta);
    return C;
}

inline Eigen::Vector2d calcFi(double vx, double vy, double w, double px, double py, double theta, double vp)
{
    Eigen::Vector2d F << vx * std::cos(theta) + vy * std::sin(theta) + (px * std::sin(theta) - py * std::cos(theta)) * w - vp,
                        vx * std::sin(theta) + vy * std::cos(theta) + (px * std::cos(theta) - py * std::sin(theta)) * w;
}

inline Eigen::Vector3d calcDerivateFi(Eigen::Vector2d &Ai, Eigen::Vector2d &Bi, Eigen::Vector2d &Ci, Eigen::Vector2d &Fi)
{
    Eigen::Vector3d DFi << 2 * Fi.transpose() * Ai, 2 * Fi.transpose() * Bi, 2 * Fi.transpose() * Ci;
    return DFi;
}

inline Eigen::Matrix3d calckHasen(Eigen::Vector2d &Ai, Eigen::Vector2d &Bi, Eigen::Vector2d &Ci)
{
    Eigen::Matrix3d H << Ai.transpose()*Ai, Bi.transpose()*Ai, Ci.transpose()*Ai,
                         Ai.transpose()*Bi, Bi.transpose()*Bi, Ci.transpose()*Bi,
                         Ai.transpose()*Ci, Bi.transpose()*Ci, Ci.transpose()*Ci;
    return H;
}

inline bool calcMinimumIK(Eigen::vector3d &init_condition, const std::vector<VehicleWheelState> wheel_states, 
    const high_resolution_clock::time_point& begin_time, 
    double step_rate = 0.01, double stop_thresh = 1e-8, double stop_time = 100.0)
{
    // 计算第一次的dx, dy, dw   要求设置步长step = 0.1

    Eigen::Vector2d Ai = {0, 0};
    Eigen::Vector2d Bi = {0, 0};
    Eigen::Vector2d Ci = {0, 0};
    Eigen::Vector3d Fi = {0, 0, 0};

    Eigen::Vector3d DF = {0, 0, 0};
    Eigen::Matrix3d H = {0, 0, 0, 0, 0, 0, 0, 0, 0};

    for(auto &wheel : wheel_states)
    {
        Ai= calcAi(init_vx, init_vy, init_w, 
                wheel.getPostion().getX(), wheel.getPosition().getY(), wheel.getSteerAngle(), 
                wheel.getWheelLinearSpeed());
        Bi = calcBi(init_vx, init_vy, init_w, 
                wheel.getPostion().getX(), wheel.getPosition().getY(), wheel.getSteerAngle(), 
                wheel.getWheelLinearSpeed());
        Ci = calcCi(init_vx, init_vy, init_w, 
                wheel.getPostion().getX(), wheel.getPosition().getY(), wheel.getSteerAngle(), 
                wheel.getWheelLinearSpeed());
        Fi = calcFi(init_vx, init_vy, init_w, 
                wheel.getPostion().getX(), wheel.getPosition().getY(), wheel.getSteerAngle(), 
                wheel.getWheelLinearSpeed());
        
        DF+= DF + calcDerivateFi(Ai, Bi, Ci, Fi);
        H+=calckHasen(Ai, Bi, Ci);
    }

    init_condition = init_condition - H.inverse() * DF ;
    if (DF.norm() <= fabs(stop_thresh))
    {
        return true;
    }

    high_resolution_clock::time_point end_time = high_resolution_clock::now();
    milliseconds timeInterval = std::chrono::duration_cast<milliseconds>(end_time - begin_time);
    if (timeInterval.count() > stop_time)
    {
        return false;
    }

    // 迭代计算
    return calcIK(init_vx, init_vy, init_w, wheel_states, begin_time);
}

} // namespace name
